#include <ros/ros.h>
#include <fstream>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

#include <tf/transform_broadcaster.h>

#include "eigen_conversions/eigen_msg.h"
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include "optitrack_uav_operator/optitrack_uav_operator.h"
#include "optitrack_uav_operator/uav_leader.h"
#include <boost/thread.hpp>

using namespace Eigen;
using namespace std;

namespace px4_operator_group_flight_demo
{

}


void command_thread_function()
{

}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "group_flight_demo");
    ros::NodeHandle nh;



    const int MAX_UAV_NUM = 5;

    int uav_num = 1;
    //if(!ros::param::get("~uav_num", uav_num))
   // {
   // 	ROS_ERROR("must set uav number _uav_num:=[num]");
    //	return 1;
   // }
    string rigid_body_header="Tracker11";
    if(!ros::param::get("~rigid_body_header", rigid_body_header))
	{
		ROS_ERROR("must set rigid body header _rigid_body_header:=[****]");
		//return 1;
	}
    string mavros_namespace_header;
	//if(!ros::param::get("~mavros_namespace_header",mavros_namespace_header))
	//{
	//	ROS_ERROR("must set mavros_namespace_header _mavros_namespace_header:=[****]");
	//	return 1;
	//}

	UAVLeader uav_leader;
	bool init_success = uav_leader.init(nh, uav_num, rigid_body_header, mavros_namespace_header);
	if(init_success)
	{
		ROS_INFO("UAV leader init successful");
	}
	else
	{
		ROS_ERROR("UAV leader init failed");
		return 1;
	}

    const double MAIN_CYCLE_RATE = 60.0;
    const double MAIN_CYCLE_INTERVAL = 1.0 / MAIN_CYCLE_RATE;
    ros::Rate main_rate(MAIN_CYCLE_RATE);

    ros::Rate wait_connection_rate(10.0);
    //boost::thread command_thread(command_thread_function);



    while(ros::ok() ){
        ros::spinOnce();
        uav_leader.spinonce();

        if(uav_leader.all_connected)
        {
        	ROS_INFO("All uavs connected");
        	break;
        }
        else
        {
        	ROS_WARN_THROTTLE(1.0, "Waiting for all uavs to be connected");
        	//break;
        }

        wait_connection_rate.sleep();
    }

    for(int i=0;i<60;i++)
    {
	  ros::spinOnce();
      uav_leader.spinonce();
      main_rate.sleep();
    }


    double start_time = ros::Time::now().toSec();
    fd_set fds;
	FD_ZERO(&fds);
	FD_SET(STDIN_FILENO,&fds);




	char stdin_buffer[256];

    while(ros::ok())
    {
    	uav_leader.spinonce();
    	uav_leader.command_uavs(ros::Time::now().toSec());

		ros::spinOnce();
		main_rate.sleep();

		fflush(stdin);
		char keyboard_hit = 0;

		timeval tv;
		tv.tv_sec = 0;
		tv.tv_usec = 1000*1;
		FD_ZERO(&fds);
		FD_SET(STDIN_FILENO,&fds);

		int select_ret = select(STDIN_FILENO+1,&fds,NULL,NULL,&tv);
		if(select_ret <0)
		{
			ROS_INFO("error in select stdin");
			ros::shutdown();
		}
		else
		{
			if(!FD_ISSET(STDIN_FILENO, &fds))
			{
				//printf("no key \n");
				continue;
			}
		}

		read(STDIN_FILENO,stdin_buffer,256);
		keyboard_hit = stdin_buffer[0];
		//printf("keyboard: %d \n", keyboard_hit);
		if(keyboard_hit == 'a')
		{
			ROS_INFO("request arm");
			uav_leader.set_mission(UAVLeader::Mission::ARMED_STANDBY);
		}
		else if(keyboard_hit == 'd')
		{
			if(stdin_buffer[1] == '!')
			{
				ROS_INFO("request force disarm");
				uav_leader.set_mission(UAVLeader::Mission::FORCE_DISARM);
			}
			else
			{
				ROS_INFO("request disarm");
				uav_leader.set_mission(UAVLeader::Mission::DISARM);
			}
		}
		else if(keyboard_hit == 't')
		{
			ROS_INFO("request takeoff");
			uav_leader.set_mission(UAVLeader::Mission::TAKEOFF);
		}
		else if(keyboard_hit == 'l')
		{
			ROS_INFO("request land");
			uav_leader.set_mission(UAVLeader::Mission::LAND);
		}
		else if(keyboard_hit == 'i')
		{
			if(stdin_buffer[1] == '1')
			{
				ROS_INFO("request init position");
				uav_leader.set_mission(UAVLeader::Mission::INIT_POSITION);
			}
			else if(stdin_buffer[1] == '1')
			{
				ROS_INFO("request init position");
				uav_leader.set_mission(UAVLeader::Mission::INIT_POSITION);
			}
			else
			{
				ROS_INFO("request init position");
				uav_leader.set_mission(UAVLeader::Mission::INIT_POSITION);
			}
		}
		else if(keyboard_hit == 'r')
		{
			ROS_INFO("request rounding");
			uav_leader.set_mission(UAVLeader::Mission::ROUNDING);
		}
		else if(keyboard_hit == 'f')
		{
			ROS_INFO("request rounding");
			uav_leader.set_mission(UAVLeader::Mission::ROUNDING);
		}
		else if(keyboard_hit == 'h')
		{
			ROS_INFO("request hold position");
			uav_leader.set_mission(UAVLeader::Mission::LOITER);
		}
		//takeoff way can not work
		//change to the acc control------for debug!!
		else if(keyboard_hit == 'u')
		{
			ROS_INFO("request takeoff automatically!");
			uav_leader.set_mission(UAVLeader::Mission::TAKEOFF_V2);
		}
		//for the display
		else if(keyboard_hit == 'p')
		{
			ROS_INFO("request display!");
			uav_leader.set_mission(UAVLeader::Mission::DISPLAY);
		}

		else if(keyboard_hit == 'q')
		{
			ROS_INFO("Dangerous action!!!");
			uav_leader.set_mission(UAVLeader::Mission::GOTHROUGH);

		}


		else
		{
			ROS_INFO("unknown command");
		}

	}


    return 0;
}
